import z3
import numpy as np
from planner_utils import *

n_steps = 14 # number of interpolating points on trajectory, number of steps
max_step_length = 0.5 # maximum length of each step
n_substeps = 10  # ensure that max_step_length/n_substeps << size_of_obs


x = [0]
y = [0]
for i in range(n_steps):
    x.append(z3.Real('x'+str(i)))
    y.append(z3.Real('y'+str(i)))
x.append(0)
y.append(3)

tactic = z3.Then('simplify', 'qflra')
solver = tactic.solver()
solver.add(step_length_constraint(x, y, max_step_length))
for t in np.linspace(0,1,n_substeps):
    solver.add(free_constraint(x, y, t))

r = solver.check()
if r == z3.sat:
    model = solver.model()
    for i in range(len(x)):
        print_var(x[i], model)
        print_var(y[i], model)
else:
    print(r)
